package com.example.myfirstgame.helper;


public class KalmanFilter {
		float q; //process noise covariance
		int r; //measurement noise covariance
		float x; //value
		float p; //estimation error covariance
		float k; //kalman gain

		public KalmanFilter( float q, int r, float x, float p, float k){
		     this.q = q;
		     this.r = r;
		     this.x = x;
		     this.p = p;
		     this.k = k;
		}
		public KalmanFilter(){
			 this.q = 0.02f;
		     this.r = 15;
		     this.x = 1;
		     this.p = 1.38f;
		     this.k = 0.05f;
	    }      
		//Calculataion
		public float kalman_update(float measurement)
		{
		  //prediction update
		  //omit x = x
		  p = p + q;
		 
		  //measurement update
		  k = p / (p + r);
		  x = x + k * (measurement - x);
		  p = (1 - k) * p;
		  return (float) x;
		}
		
		public void setParams(float q, int r, float x, float p, float k){
			this.q = q;
		    this.r = r;
		    this.x = x;
		    this.p = p;
		    this.k = k;
		}
}
